#include "rclcpp/rclcpp.hpp"
#include "control/WMJRobotControl.h"
#include "std_msgs/msg/u_int8.hpp"
namespace wmj
{
    void Control::publishRobotMessage()
    {
        uint8_t recv = RobotControl->recvFromRobot();
        auto msg = base_interfaces::msg::Communicate();
        msg.robot_recv = recv;
        Control::m_recvRobotMsgPublisher->publish(msg);
    }

    void Control::sendRobotMsgCallback(const base_interfaces::msg::Communicate::SharedPtr msg)
    {
        int8_t Robo_id = msg->id.value;
        uint8_t content_1 = msg->content;
        enum ROBO_ID ID;
        if (Robo_id == 0)
            ID = ID_HERO;
        if (Robo_id == 1)
            ID = ID_ENGINERR;
        if (Robo_id == 2)
            ID = ID_INFANTRY3;
        if (Robo_id == 3)
            ID = ID_INFANTRY4;
        if (Robo_id == 4)
            ID = ID_INFANTRY5;
        RobotControl->sendToRobot(ID, content_1);
    }

}